/*
 * Copyright 2012 Alain Pannetrat (L1L1@gmx.com)
 * 
 * This file is part of cookie-miner.
 * 
 * cookie-miner is free software: you can redistribute it and/or modify 
 * it under the terms of the GNU General Public License as published by 
 * the Free Software Foundation, either version 3 of the License, or 
 * (at your option) any later version.
 * 
 * cookie-miner is distributed in the hope that it will be useful, but 
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License 
 * along with cookie-miner. If not, see http://www.gnu.org/licenses/.
 *
 * @version: $Id$
 *
 */
#include "graphs.h"
#include "quadtree.h"
#include "log_facility.h"


/********************************************************************/
/********************************************************************
 *** 	IMPLEM.
 ********************************************************************/
/********************************************************************/

/********************************************************************
 *
 *	VERTEX
 *
 */

Vertex* Vertex::create(Universe& u, const Vector& the_pos, const std::string& the_name)
{
	return new Vertex(u,the_pos,the_name);
}

Vertex::Vertex(Universe& u, const Vector& the_pos, const std::string& the_name) :
	_universe(u),
	_name(the_name),
	_velocity(0),
	_force(0),
	_color(1,1,1), 
	_radius(5),
       	_selected(false),
	_tagged(false)
{
	_graph = new Graph(_universe,this);
	//_graph->push_back(this);
	_universe._graphs.insert(_graph);
	_universe._vertex_names.set(_name,this);
	position(the_pos);
}

Vertex::~Vertex()
{
	/* FIXME: remove vertex for real */
}

void Vertex::position(const Vector& pos)
{
	if (pos[0]<_universe._x1) 	_position[0]=_universe._x1;
	else if (pos[0]>_universe._x2) _position[0]=_universe._x2;
	else _position[0]=pos[0];
	
	if (pos[1]<_universe._y1) 	_position[1]=_universe._y1;
	else if (pos[1]>_universe._y2) _position[1]=_universe._y2; 
	else _position[1]=pos[1];
	
	_position[2]=0;
}

float Vertex::animate(float timestep) 
{
    float k_energy;

    // if things get out of hand: reduce the force //
    while (_force.abs_square()>1000000) _force *= 0.5;

    k_energy = _velocity.abs_square(); 

    if (_universe.energy()<1)
	_velocity = (_velocity+timestep*_force)*0.75;
    else
	_velocity = (_velocity+timestep*_force)*0.9;
    

    _position += _velocity;

    if (_position[0]<_universe._x1) 
    {
	_position[0]=_universe._x1-fmod(_position[0]-_universe._x1,_universe._x2-_universe._x1);
	if (_velocity[0]<0) _velocity[0]=-_velocity[0];
    }
    else if (_position[0]>_universe._x2)
    {
	_position[0]=_universe._x2-fmod(_position[0]-_universe._x1,_universe._x2-_universe._x1);
	if (_velocity[0]>0) _velocity[0]=-_velocity[0];
    }

    if (_position[1]<_universe._y1) 
    {
	_position[1]=_universe._y1-fmod(_position[1]-_universe._y1,_universe._y2-_universe._y1);
	if (_velocity[1]<0) _velocity[1]=-_velocity[1];
    }
    else if (_position[1]>_universe._y2)
    {
	_position[1]=_universe._y2-fmod(_position[1]-_universe._y1,_universe._y2-_universe._y1);
	if (_velocity[1]>0) _velocity[1]=-_velocity[1];
    }

    return k_energy;
}

void Vertex::selected(bool sel)
{
	if (_selected && !sel) 
		_universe._selected_count--;
	else if (!_selected && sel) 
		_universe._selected_count++;
	_selected = sel;
}

bool Vertex::has_edge_to(const Vertex *dest) const
{
	for (EdgeList::const_iterator it=e_begin(); it!=e_end(); ++it)
	{
		if ((*it)->destination()==dest) return true;
	}
	return false;
}

/********************************************************************
 *
 *	EDGE
 *
 */

Edge* Edge::create(Vertex* source, 
		   Vertex* destination, 
		   const Vector& color)
{
	return new Edge(source,destination,color);
}

Edge::Edge(Vertex* source, 
	   Vertex* destination,
	   const Vector& color) :
	_source(source), 
	_destination(destination), 
	_color(color) 
{
    if (_source->_graph != _destination->_graph)
    {
	if (_source->_graph->order()<_destination->_graph->order())
	    _destination->_graph->merge(_source->_graph);
	else
	    _source->_graph->merge(_destination->_graph);
    }
    _source->_edges.push_back(this);
}

/********************************************************************
 *
 *	GRAPH
 *
 */

Graph::Graph(Universe &u, Vertex* v) :
	_universe(u),
	_qn(NULL)
{
    if (v) v_add(v);
}

Graph* Graph::merge(Graph *ext)
{
	VertexList::iterator vi;
	EdgeList::iterator ei;

	for (vi=ext->v_begin(); vi!=ext->v_end(); ++vi) v_add(*vi);

	_universe._graphs.erase(ext);
	delete ext;
	return this;
}

typedef struct {
        Vertex *origin;
        Vector net_force;
        VFloat kd_2;
} vertex_info_t;

#define THETA_2 (0.8*0.8)
bool net_force_compute(QNode* qnode, void *data)
{
        Vector r;
        VFloat square_norm;
        vertex_info_t *vinfo = (vertex_info_t *)data;

        if (qnode->vertex()==vinfo->origin) return false; // Comparing with ourselves

        r = vinfo->origin->position()-qnode->position();
        square_norm = r.abs_square();

        if (square_norm<1) square_norm = 1;

        if (qnode->vertex()!=NULL || (qnode->surface()/square_norm)<THETA_2)
        {
                vinfo->net_force += qnode->weight() * vinfo->kd_2/square_norm * r;
                return false;
        }
        return true;
}


void Graph::apply_force(const Vector& external_force)
{
    VFloat kd = _universe.kd_parameter();
    VFloat kd_2 = kd*kd;
    vertex_info_t vinfo;

    vinfo.kd_2 = kd_2;

    for (VertexList::iterator vit=v_begin(); vit!=v_end(); ++vit)
    {
	Vertex *node = *vit;
	
	if (node!=_universe._suspended_vertex)
	{
	    vinfo.net_force = external_force;
	    vinfo.origin = node;

	    _qn -> traverse(net_force_compute,&vinfo);


	    node->force(vinfo.net_force);
	}
	else 
	    node->force(Vector::null);

    }

    for (VertexList::iterator vit=v_begin(); vit!=v_end(); ++vit)
    {
	Vertex *node = *vit;
	
	for (EdgeList::iterator eit=node->e_begin(); eit!=node->e_end(); ++eit)
	{
	    Vertex *neighbor = (*eit)->destination();
	    Vector r = node->position() - neighbor->position();
	    Vector net_force = r.norm()/kd * r;

	    node->force(node->force() - net_force);
	    neighbor->force(neighbor->force() + net_force);
	}
    }
}


inline float pright(const Vector& v) 	{ return (v[0]+v[1]); }
inline float pleft(const Vector& v) 	{ return (v[0]-v[1]); }

void Graph::update_quadtree()
{
	if (_qn) delete _qn;

	_universe.get_limits(_x2,_y2,_x1,_y1);
	_d1 = _x1+_y1;
	_d2 = -_y1;
	_d3 = 0;
	_d4 = _x1;

	_qn = QNode::create(_x2,_y2,_x1,_y1);

	for (VertexList::const_iterator it=v_begin(); it!=v_end(); ++it)
	{
		Vector v = (*it)->position();

		if (v[0]<_x1) _x1=v[0];
		if (v[0]>_x2) _x2=v[0];
		if (v[1]<_y1) _y1=v[1];
		if (v[1]>_y2) _y2=v[1];
		if (pright(v)<_d1) _d1 = pright(v);
		if (pright(v)>_d3) _d3 = pright(v);
		if (pleft(v) <_d4) _d4 = pleft(v);
		if (pleft(v) >_d2) _d2 = pleft(v);

		_qn->add(*it);
	}
}

bool Graph::intersects(Graph* with_graph) const
{
	if (_x2<with_graph->_x1-10) return false;
	if (_x1>with_graph->_x2+10) return false;
	if (_y2<with_graph->_y1-10) return false;
	if (_y1>with_graph->_y2+10) return false;

	if (_d3<with_graph->_d1-10) return false;
	if (_d1>with_graph->_d3+10) return false;
	if (_d2<with_graph->_d4-10) return false;
	if (_d4>with_graph->_d2+10) return false;
	
	return true;
}

Graph::~Graph() 
{ 
	if (_qn) delete _qn; 
} 

/********************************************************************
 *
 *	UNIVERSE
 *
 */

Universe::Universe(float x1, float y1, float x2, float y2) :
	_k_energy(0),
	_suspended_vertex(NULL), 
	_selected_count(0)
{ 
	set_limits(x1,y1,x2,y2); 
}

float Universe::animate(float timestep)
{
	float kd = kd_parameter();
	float kd2 = kd * kd;

	for (GraphSet::iterator git = g_begin(); git!=g_end(); ++git)
	{
		(*git)->update_quadtree();
	}

	_k_energy = 0;
	
	for (GraphSet::iterator git1 = g_begin(); git1!=g_end(); ++git1)
	{
	    Vector force(0);
	    Vector c1((*git1)->_qn->position());

	    for (GraphSet::iterator git2 = g_begin(); git2!=g_end(); ++git2) if ((*git1)!=(*git2))
	    {
		if ((*git1)->intersects(*git2))
		{
		    Vector c2((*git2)->_qn->position());
		    Vector r = c1-c2;

		    force += (*git2)->_qn->weight() * kd2/r.abs_square()*r;
		}
	    }

	    (*git1)->apply_force(force);
	}

	for (GraphSet::iterator git1 = g_begin(); git1!=g_end(); ++git1)
	{
	    for (VertexList::iterator vit=(*git1)->v_begin(); vit!=(*git1)->v_end(); ++vit)
	    {
		if ((*vit)!=_suspended_vertex) _k_energy += (*vit)->animate(timestep);
	    }
	}
	
	return _k_energy;
}


void Universe::set_limits(float x1, float y1, float x2, float y2)
{
	_x1 = x1;
	_y1 = y1;
	_x2 = x2;
	_y2 = y2;
}

void Universe::get_limits(float& x1, float& y1, float& x2, float& y2) const
{
	x1 = _x1;
	y1 = _y1;
	x2 = _x2;
	y2 = _y2;
}

Vertex* Universe::vertex_locate(const Vector& pos, float tolerance) const
{
	for (GraphSet::const_iterator git = g_begin(); git!=g_end(); ++git)
	{
	    for (VertexList::const_iterator vit=(*git)->v_begin(); vit!=(*git)->v_end(); ++vit)
	    {
		Vertex *v = (*vit);
		float limit = (v->radius()+tolerance)*(v->radius()+tolerance);
		Vector diff = v->position()-pos;

		if (diff.abs_square()<=limit) return v;
	    }
	}
        return NULL;
}

float Universe::kd_parameter() const
{
	return (1.0/3.0) * sqrt((_x2-_x1)*(_y2-_y1)/_vertex_names.size());
}

